perm filename PARM.PAL[PNT,HE] blob
sn#619240 filedate 1981-10-19 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00016 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 put locations into comtab
C00004 00003 routines to set up coefficients for drive
C00011 00004 ROUTINE TO READ RAW FORCE WRIST AND RETURN INFORMATION TO PDP10
C00013 00005 correspondence between routines
C00015 00006 OUTSTX,scamul,addvec
C00018 00007 HLDPUM,UNCALIB
C00019 00008 KBDRTN :for calling keyboad routines
C00021 00009 CRTRTN: Keyboard controller in cartesian mode
C00026 00010 [CRTRTN: CONTINUATION]
C00030 00011
C00035 00012 JNTRTN: Joint motions via the keyboard.
C00040 00013 [JNTRTN: CONTINUATION]
C00044 00014 FRERTN: Frees up a joint
C00046 00015 GRARTN: Gravity model experimentation.
C00050 00016 DATA
C00051 ENDMK
C⊗;
;put locations into comtab
DATA
PUTLOC LSETEXA,EXASAV
PUTLOC LRFORCE, RFORCE
PUTLOC LDRV0,DRV0
PUTLOC LDRIVE,DRIVE
PUTLOC LUSTPPTR,USTPPTR
PUTLOC LLSTPPTR,LSTPPTR
PUTLOC LUNCALIB,UNCALIB
USTPPTR:
YUSTOP: YELPTR USTOP ;POINTERS YELLOW ARM JOINT ANGLE UPPER LIMITS
BUSTOP: BLUPTR USTOP ; " BLUE " " " " "
HUSTOP: HDVPTR USTOP ; " HARDWARE " " " "
LSTPPTR:
YLSTOP: YELPTR LSTOP ;POINTERS YELLOW ARM JOINT ANGLE LOWER LIMITS
BLSTOP: BLUPTR LSTOP ; " BLUE " " " " "
HLSTOP: HDVPTR LSTOP ; " HARDWARE " " " "
CODE
; routines to set up coefficients for drive
; DRV0 is a routine that computes the appropriate coefficients
; given the arm and joint number and the amount of motion and
; whether it is absolute or relative.
; AC0 = some amount
; r3 = joint number
; r1 = arm number
; r2 = absolute (1) or relative (0)
⊗
DRV0: MOV R4,-(SP)
MOV R5,-(SP)
MOV R0,CFLST ; save coeff list address
MOV R3,JOINT
MOV R0,R3 ; r3 now will build up polylist
MOV R2,ABSDRV ; save r2
STF AC0,JTAMT ; save r0
TST R1 ; check which arm
BEQ 1$ ; skip if yellow
MOV #4,R0 ;THIS IS BLUE ARM JOINT 7 SERVO BIT
BR 2$
1$: MOV #1000,R0 ;THIS IS YELLOW ARM JOINT 7 SERVO BIT
2$: MOV #7,R1 ;R1←7
SUB JOINT,R1 ;R1←7-JOINT NUMBER
ASH R1,R0 ;SHIFT BIT TO INDICATE PROPER JOINT
MOV R0,(R3)+ ;SET SERVO BIT
CLR (R3)+ ;zero next servo word
CLR (R3)+ ;no special word
CLR (R3)+ ;no wobble
MOV #40.,(R3)+ ;rel segment pointer
MOV #2000.,(R3)+ ;SET DURATION OF DRIVE MOTION = 2000 MSEC
CLR (R3)+ ; no trans
CLR (R3)+ ;no run code
MOV R3,-(SP) ; save the address so far
;SET UP THE DYNAMIC COEFFICIENT LIST CI AND CII
BIT #YARM+YHAND,@CFLST ;CHECK IF THIS IS FOR THE YELLOW ARM
BEQ NOYEL
MOV #YARM+YHAND,WLST ;IF SO, READ THE CURRENT JOINT ANGLES
MOV #YELARM,R2 ;INDICATE YELLOW ARM
MOV #YTH,R4 ;GET POINTER TO YELLOW JOINT ANGLES
BR FILLAN
;*k could be vise too
NOYEL: MOV #BARM+BHAND,WLST ;ELSE MUST BE BLUE ARM
MOV #BLUARM,R2 ;INDICATE BLUE ARM
MOV #BTH,R4 ;GET POINTER TO BLUE JOINT ANGLES
FILLAN: MOV #WLST,R0
MOV #DEVICE,R1
JSR PC,WHERE
BIT #YHAND+BHAND,@CFLST ;CHECK IF HAND SELECTED FOR DRIVE
BNE ISHND ;BRANCH IF HAND OPERATION
MOV JOINT,R3 ;GET POINTER TO JOINT ANGLE
DEC R3
ASH #1,R3
ADD R4,R3
LDF JTAMT,AC0
LDF @(R3),AC1 ;AC1←OLD VALUE
STF AC1,JTOLD ;save old value of joint
TST ABSDRV
BEQ 2$ ; branch because relative
SUBF AC0,AC1 ;AC1←DIFFERENCE
NEGF AC1
STF AC1,JTDIF
BR 3$
2$: STF AC0,JTDIF
3$: MOV R4,R0 ;COMPUTE CORRESPONDING ARM DYNAMIC COEFFICIENTS
MOV #CIPTR,R1
JSR PC,DTERMS
MOV JOINT,R3 ;GET INDEX TO DYNAMIC COEFFICIENTS
ASH #2,R3
LDF CILST-4(R3),AC0 ;PICK UP GRAVITY LOADING AND INERTIA
STF AC0,SEGCI
LDF CIILST-4(R3),AC0
STF AC0,SEGCII
MOV #WLST,R0 ;RESTORE CURRENT JOINT ANGLES
MOV #DEVICE,R1
JSR PC,WHERE
BR DODRVE
ISHND:
BIT #YHAND,@CFLST ;CHECK IF YELLOW HAND
BEQ NOYHND
LDF CI+SRV07,AC0 ;IF YELLOW GET PROPER CI AND CII
LDF CII+SRV07,AC1
BR DOHAND
NOYHND: BIT #BHAND,@CFLST ;CHECK IF BLUE HAND
BEQ NOBHND
LDF CI+SRV14,AC0 ;IF BLUE GET PROPER CI AND CII
LDF CII+SRV14,AC1
BR DOHAND
NOBHND: BIT #VISE,@CFLST ;CHECK IF VISE
BEQ NOVISE
; LDF CI+SRV15,AC0 ;IF VISE GET PROPER CI AND CII
; LDF CII+SRV15,AC1
BR DOHAND
;;; should not come here
NOVISE:
;; MOV #NSCR,R0 ;SAY LOOKING FOR SCREWDRIVER
;; JSR PC,TYPSTR
;; DISMIS
DOHAND: STF AC0,SEGCI ;PUT DYNAMIC COEF. IN DATA LIST
STF AC1,SEGCII
;fill up rest of coefficients
DODRVE: MOV (SP)+,R3 ;restore state of coef list
MOV #6,R1
4$: CLR (R3)+ ;next tHREE coeffs=0
SOB R1,4$
LDF JTDIF,AC0 ;GET THE CHANGE IN JOINT ANGLE
LDF C10,AC1 ;SCALE 5TH ORDER POLYNOMIAL BY CHANGE AND PACK
MULF AC0,AC1 ; IN TO COEFFICIENT DATA LIST
STF AC1,(R3)+
LDF CM15,AC1
MULF AC0,AC1
STF AC1,(R3)+
LDF F6,AC1
MULF AC0,AC1
STF AC1,(R3)+
LDF SEGCI,AC0
STF AC0,(R3)+
LDF SEGCII,AC0
STF AC0,(R3)+
CLR (R3)+
CLR (R3)+
MOV (SP)+,R5
MOV (SP)+,R4
RTS PC
DATA
ABSDRV: .WORD 0
JOINT: .WORD 0
JTDIF: .WORD 0
DEVICE: .BLKW 32 ;DEVICE BLOCK
CFLST: .WORD 0 ;coefficient list
SEGCI: .BLKW 2 ;CI
SEGCII: .BLKW 2 ;CII
WLST: 374 ;JOINT SERVO BIT
0
C10: .WORD 41040,0 ;10.0
CM15: .WORD 141160,0 ;-15.0
F6: .WORD 40700,0 ;6.0
;TABLES FOR COMPUTE DYNAMIC COEFFICIENTS
CIPTR: CILST
CILST+4
CILST+10
CILST+14
CILST+20
CILST+24
CIILST
CIILST+4
CIILST+10
CIILST+14
CIILST+20
CIILST+24
CILST: .BLKW 12.
CIILST: .BLKW 12.
JTOLD: .WORD 0
JTAMT: .WORD 0,0
NSCR: .ASCIZ /LOOKING FOR THE SCREWDRIVER
/
CODE
;ROUTINE TO READ RAW FORCE WRIST AND RETURN INFORMATION TO PDP10
; copied over from old POINTY file
; note that the data sent is in integer format
;
; procedure should be called with R1 containing a pointer to
; the buffer in which the data is to be stored.
; sample call is as follows:
;
; MOV #FPPTR,R1
; JSR PC,RFORCE
;
; R0,R2 will be garbaged.
; FPPTR will be updated
;
RFORCE: MOV R3,-(SP) ;save registers
MOV R4,-(SP)
MOV R1,-(SP)
MOV (R1),R1 ;now R1 has address instead of address of address
MOV #10.,R2 ;READ 10 SETS OF DATA
SETLP: MOV #9.,R3 ;EIGHT STRAIN GAGES IN ALL+REF CHAN
MOV #30,R4 ;FIRST STRAIN GAGE CHANNEL
REDLP: MOVB R4,ADCCHN ;START CONVERTING STRAIN GAGE READING
WLP: TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BMI CNVDNE
BR WLP
CNVDNE: MOV ADCVAL,R0 ;GET READING FROM BLUE INTERFACE
; ADD #2048.,R0
MOV R0,(R1)+ ;SAVE READING
INC R4 ;POINT TO NEXT CHANNEL
SOB R3,REDLP ;REPEAT UNTIL DONE
CLR -2(R1) ;NO REFERENCE READING
SOB R2,SETLP ;DO IT 10. TIMES
MOV (SP)+,R0 ;now update the pointer value
MOV R1,(R0)
MOV (SP)+,R4 ;retrieve registers
MOV (SP)+,R3
RTS PC
COMMENT ⊗ correspondence between routines
ARM.PAL WAVARM.PAL[PUM,HE],WAVSUP.PAL[PUM,HE],ETC
MOVPUM MOVEIT
PANGLE ANGLES
UPDATE WHERE
SOLVE SOLVE
EVPUMA MOVARM
BITON/OFF BITON/OFF
WVECT,RVECT WVECT,RVECT
⊗ ;
DATA
PUTLOC LKBDRTN,KBDRTN
CODE
; OUTSTX,scamul,addvec
.MACRO OUTSTX B ;type string starting at B
MOV #B,R0 ;Load up the string to be output
JSR PC,@LTYPSTR ;Call the string output utility routine
.ENDM
; INCHRI looks at the terminal. If a char is there, it gets put in R0, else
; R0 is cleared to mean no char yet. This differs from the INCHR in
; ALIO in that this does not wait for a character, and the character is
; returned in R0. It also echoes the character
INCHRI: MFPD OUTSW ;Where does it come from?
TST (SP)+
BEQ 1$
TSTB KBIS ;Anything typed on VT05?
BPL 2$ ; No
MOVB KBIR,R0 ; Read the char
BIC #177600,R0 ;Clear all but low 7 bits
JSR PC,@LTYPCHR ;Type the character
RTS PC
1$: MFPD IREG ;Anything from the 10?
MOV (SP)+,R0
BEQ 2$ ; No
CLR -(SP)
MTPD IREG ; Fetch the char
BIC #177600,R0
RTS PC
2$: CLR R0 ;No input
RTS PC
;SCAMUL performs a scalar multiply: multiplies the vector (R0) by AC0.
SCAMUL: PUSHF AC1
PUSH R5
MOV #3,R5
1$: LDF (R0),AC1 ;Get component
MULF AC0,AC1 ;and multiply by the scalar
STF AC1,(R0)+ ;Store back
SOB R5,1$
POP R5
POPF AC1
RTS PC
;ADDVEC adds the vector (R0) to (R1).
ADDVEC: PUSHF AC0
PUSH R5
MOV #3,R5 ;Vector has 3 components
1$: LDF (R1),AC0 ;Get component of 2nd vector
ADDF (R0)+,AC0 ;Add component of 1st to it
STF AC0,(R1)+ ; and store back
SOB R5,1$
POP R5
POPF AC0
RTS PC
; HLDPUM,UNCALIB
HLDPUM: MOV DANGLE,R0 ;reads current puma position and holds it there
MOV SRVDAT,DAT
JSR PC,PANGLE ;update the angles
PUSH <R2,R3,R4,R5>
MOV DANGLE,R0 ; go back to position mode with desired position
MOV SRVDAT,DAT
JSR PC,MOVPUM
POP <R5,R4,R3,R2>
RTS PC
UNCALIB:CLR CALFLG+SRV17 ;Clears "calibrated" flags for both Puma's
CLR CALFLG+SRV19
RTS PC
;KBDRTN :for calling keyboad routines
KBDRTN: MOV R1,MECHNO
CMP R1,#GRNARM ; find appropriate index for table pointer
BEQ 2$ ; is it garm?
MOV #2,R1 ; no, it must be red
BR 3$
2$: CLR R1 ; yes, it's green
3$: MOV DDANGLE(R1),DANGLE ; set appropriate values in dangel and srvdat
MOV DSRVNUM(R1),SRVDAT
ADD R0,R0
MOV 1$(R0),R0 ; get address of routine to jump to
PUSH <R2,R3,R4>
MOV EXACT,EXASAV
MOV #1,EXACT ; to allow different modes of arm solution
; to have closest orientation etc
JSR PC,(R0)
MOV EXASAV,EXACT ; as it was
POP <R4,R3,R2>
RTS PC
DATA
1$: .WORD FRERTN ; free it
.WORD JNTRTN ; joint control
.WORD TBLRTN ; cartesian control
.WORD TULRTN ; tool control
.WORD GRARTN ; gravity free
DDANGLE:.WORD GTH,RTH
DMECHNO:.WORD GRNARM,REDARM
DSRVNUM:.WORD SRV17,SRV19
EXASAV: .WORD 0 ; save value of exact and set it up
CODE
; CRTRTN: Keyboard controller in cartesian mode
TBLRTN: OUTSTX KBDMS1
OUTSTX KBDWRL
CLR KMODE ; KMODE←0
BR CRTRTN
TULRTN: OUTSTX KBDMS1
OUTSTX KBDTUL
MOV #2,KMODE ; KMODE←2
CRTRTN: CLR MOVFLG ;1=ARM IS MOVING
CLR CHRCNT ;RESET # OF CHARS TYPED.
LDF FLT2,AC0 ;Re-calculate linear length in case it got changed
DIVF CYCRAT,AC0 ; LinearLength = LinearSpeed/CycleRate
STF AC0,LINLEN
KGET0: MOV DANGLE,R0 ;Where to put joint angles
MOV SRVDAT,DAT ;
JSR PC,PANGLES ;Read encoders, convert to angles, store in DANGLE
MOV #JTRES,R0 ;R1 points to result.
MOV #THPTR,R1 ;Prepare for call to solution routine. R0 ← angles
MOV MECHNO,R2
JSR PC,UPDATE ;Get backwards solution.
KGET: JSR PC,INCHRI ;Any new command typed?
TST R0
BEQ 50$ ;If not, br
INC CHRCNT ;ADD TO # OF CHARS ON THE LINE
CMP CHRCNT,CHRMAX
BLE 3$
PUSH R0
OUTSTX CRLFX
CLR CHRCNT
POP R0
3$: MOV #KBDTBL,R1 ;SET R1 TO 1ST ENTRY IN CMD TABLE
MOVB R0,KBDEND ;SET LAST ENTRY TO THE CMD LETTER (FOR A TRAP)
5$: CMPB (R1),R0 ;DOES IT MATCH THE INPUT?
BEQ 10$ ;IF SO, QUIT
ADD #6,R1 ;TO NEXT ENTRY
BR 5$
10$: MOV 2(R1),R0 ;GET ADDR OF DIRECTION VECTOR OR ROUTINE
TST R0 ;IF NOT FOUND IN TABLE
BGE 20$ ;THEN COMPLAIN NOW
OUTSTX WHAT
CLR MOVFLG ;ARM IS NOT MOVING ANY MORE
BR KGET
20$: TST 4(R1) ;IS THIS A MOTION COMMAND?
BNE 30$ ;YES - GO COPY DIRECTION VECTOR
JMP (R0) ;NO - GO TO ROUTINE
30$: MOV KMODE,R0
MOV DIRADR(R0),R0 ;R0 has address of appropriate vector
ADD 2(R1),R0 ;add offset into vector table to get direction
MOV 4(R1),R2 ; (save +/- sense of vector in R2)
MOV #DIREC,R1 ;COPY DIRECTION VECTOR
MOV #3,R3
JSR PC,CPYFLT
LDF LINLEN,AC0 ; linear segment to get segment components.
TST R2 ; motion in +ve direction
BGT 40$
NEGF AC0 ;negative motion
40$: MOV #DIREC,R0 ; multiply direction vector by length of each deg
JSR PC,SCAMUL
INC MOVFLG ;MEANS ARM IS MOVING NOW.
BR KDOIT ;Go move the arm.
50$: TST MOVFLG ;IS THE ARM MOVING? IF NOT,
BEQ KGET ; then keep look till we get a command
KDOIT: MOV #DIREC,R0 ;Now add increment vector to current position
MOV #JTRES+PX,R1 ; set 2nd operand = P vector
JSR PC,ADDVEC ; add two vectors (result into P)
MOV MECHNO,R2
MOV #THPTR,R1
MOV #JTRES,R0
JSR PC,SOLVE ; now go do an arm solution
TSTB R0 ;SOL'N FOUND?
BNE 5$ ;If not, br & report
BIT #FARBIT,R0 ;GOT FAR SOLUTION??
BEQ 1$ ;IF CLOSE SOL'N, BR - OK
OUTSTX WATCH ;SAY GOT FAR SOL'N
JMP KSTOP
5$: BIC #177400,R0 ;CLEAR HIGH BITS IF SET.
; BIS #NOSOLN,R0 ;NO, TYPE BAD JOINT NUMBER
OUTSTX BADSOL ;PRINT ERROR MESSAGE - NO SOLUTION
JMP KSTOP ;STOP THE ARM BUT WAIT FOR MORE INPUT.
1$: MOV DANGLE,R0 ;SET ADDR OF ANGLES TO MOVE TO
MOV SRVDAT,DAT
JSR PC,MOVPUM ;MOVE THE ARM TO DESIRED POSITION.
11$: TST R1
BEQ 2$ ;IF ERROR DURING MOVE, QUIT
JMP KSTOP
2$: SLEEP #12 ;otherwise sleep for 10 msec
JMP KGET
; [CRTRTN: CONTINUATION]
KSLOW: LDF FLT1,AC1 ;CALCULATE 1/KFACTR
DIVF KFACTR,AC1 ;FACTOR TO MULTIPLY BY INTO AC1.
BR RECALC
KFAST: LDF KFACTR,AC1 ;MULTIPLICATION FACTOR
RECALC: LDF LINSPD,AC0 ;INCREASE THE SPEED
MULF AC1,AC0
STF AC0,LINSPD ;Store new linear speed.
DIVF CYCRAT,AC0 ; LinearLength = LinearSpeed/CycleRate
STF AC0,LINLEN
STF AC1,AC0 ;Factor to multiply LinearLength by.
MOV #DIREC,R0 ;Multiply Direction vector by length of each
CALL SCAMUL ; segment to get length of each component.
JMP KGET
KOPEN: MOV #AIROPN,R0 ;Set bit to open hand
BR KHAND
KCLOSE: MOV #AIRCLS,R0 ;Bit to close hand
KHAND: MOV SRVDAT,DAT
BIC #AIROPN+AIRCLS,ARMBIT(DAT)
CALL BITON ;Set bit in R0 on in joint 7 status word
JMP KGET
KTOOL: MOV #2,KMODE
BR KCHGM
KWORLD: CLR KMODE
KCHGM: CLR MOVFLG
JMP KGET0
KSTOP: CLR MOVFLG ;NO LONGER MOVING
JMP KGET
CQUIT: CLR R1
RTS PC ;EXIT
;CPYFLT: COPIES R3 FLOATING NUMBERS FRO (R0) TO (R1). ALL REGS USED ARE CHANGED.
CPYFLT: MOV (R0)+,(R1)+
MOV (R0)+,(R1)+
SOB R3,CPYFLT
RTS PC
DATA
KMODE: .WORD 0 ; mode of motion 0=table, 2=tool
LINLEN: .FLT2 0 ;CALCULATED
LINSPD: .FLT2 2.0 ;2 INCHES PER SECOND
DIRADR: .WORD WRLDIR,JTRES
DIREC: .BLKW <3*4>
JTRES: .BLKW <3*4*2>
KBDMS1: .ASCIZ /
KEYBOARD CONTROL MODE /
KBDTUL: .ASCIZ /(TOOL MODE): /
KBDWRL: .ASCIZ /(WORLD MODE): /
ERRMES: .ASCIZ /ERROR FOUND
/
FARMSG: .ASCIZ / ** TRAJ SOLN INVALID **/
WATCH: .ASCIZ / πSINGULAR SOLUTION ... /
BADSOL: .ASCIZ / π** NO SOLUTION **/
.EVEN
KBDTBL: .ASCIZ /A/ ;COMMAND = A = SLOW DOWN
.WORD KSLOW,0 ;ROUTINE ADDR & 0=NOT MOTION COMMAND.
.ASCIZ /S/ ;for a motion command, 2nd word is offset
.WORD AX,-1 ;into direction vector tbl, 3rd is +1/-1 for
.ASCIZ /D/ ;direc
.WORD OX,-1
.ASCIZ /F/
.WORD NX,-1
.ASCIZ /J/
.WORD NX,1
.ASCIZ /K/
.WORD OX,1
.ASCIZ /L/
.WORD AX,1
.ASCIZ /;/
.WORD KFAST,0
.ASCIZ /Q/
.WORD CQUIT,0
.ASCIZ / /
.WORD KSTOP,0
.ASCIZ /O/
.WORD KOPEN,0
.ASCIZ /C/
.WORD KCLOSE,0
KBDEND: .WORD 0,-1,0
KFACTR: .FLT2 1.6 ;SPEED CHANGE FACTOR
FLT2: .FLT2 2.0 ;DEFAULT SPEED, INCHES/SEC
F1000: .FLT2 1000.0
WRLDIR: .FLT2 1,0,0 ; DIRECTION FOR X-AXIS IN WORLD
.FLT2 0,1,0 ; and Y-asix
.FLT2 0,0,1 ; and Z-axis
MAXSPD: .FLT2 30.0, 35.0, 35.0, 1.00, 35.0, 35.0 ;Max speed for each joint
SPDFAC: .FLT2 1.0 ;SPEED FACTOR.
CODE
COMMENT ⊗
; MOVARM: Servoes arm to position in DANGLE
; For 5th degree poly. interolation, with speed=accel.=0 at endpoints, we use
; p = p0 + 10*del*t↑3 - 15*del*t↑4 + 6*del*t↑5
; where del=(final-initial), and t=time/T, with T=total time for move.
MOVARM: PUSH <R0,R2,R3,R4>
PUSHF AC0
PUSHF AC1
MOV #NJTS,R4 ;Figure out how many segments needed for this move
MOV #CANGLES,R0
MOV SRVDAT,DAT
JSR PC,PANGLE ;Current angles in INITH
CLR R0
CLRF AC1 ;This will be max. time for a joint to do its move
MOV DANGLE,R2 ; R2 is starting address of final angles computed
;HACK
5$: CMP R0,#14 ;Is this joint 4? If so, ignore it
BEQ 7$
LDF @(R2)+,AC0
STF AC0,FTH(R0)
SUBF INITH(R0),AC0 ;Get final-initial angle = change needed.
STF AC0,DIFF(R0) ;Store partial result (final-initial) in INCR
ABSF AC0 ; (and get absolute value of change)
DIVF MAXSPD(R0),AC0 ;Get how many seconds req'd for joint to move
COMPF AC0,AC1 ;Is time < max? If so, ignore & keep current max
BLE 7$
STF AC0,AC1 ;Save new max. time
7$: CMP (R0)+,(R0)+
SOB R4,5$
MULF SPDFAC,AC1 ;Include the speed factor too.
MULF CYCRAT,AC1 ;Multiply time by (60) to get number of segments
STCFI AC1,R4
TST R4 ;If it's zero, make it one.
BNE 8$
INC R4
8$: LDCIF R4,AC1 ;Now number of segments is an integer, in AC1
; Now move the arm. Move with number of segments in AC1 (and R4).
; Move each joint by using the polynomial
; p(t) = p0 + diff*t↑3*(10 + t*(-15 + 6*t)), with t=time/TotalTime.
10$: MOV #NJTS,R3 ;Go thru & calculate the polys for each joint
MOV DANGLE,R2
CLR R0
15$: LDCIF R4,AC0 ;Get what time it is
NEGF AC0 ;Convert to the range [0,1]
ADDF AC1,AC0
DIVF AC1,AC0 ;t is now in AC0.
STF AC0,AC3 ;Calculate 6*t
MULF FLT6,AC3
SUBF FLT15,AC3 ;AC3 = 6*t - 15
MULF AC0,AC3 ; = t(6t-15)
ADDF FLT10,AC3 ; = 10 + t(6t-15)
MULF AC0,AC3 ; = t(10 + t(6t-15))
MULF AC0,AC3 ; = t↑2(10 + t(6t-15))
MULF AC0,AC3 ; = t↑3(10 + t(6t-15))
MULF DIFF(R0),AC3 ; = diff*t↑3(10 + t(6t-15))
ADDF INITH(R0),AC3 ; = p0 + ...
STF AC3,@(R2)+ ; (store back into current angle)
CMP (R0)+,(R0)+
SOB R3,15$
; Now we know (in DANGLE) where to move the arm. So move it now.
MOV DANGLE,R0 ;Make arm move to angles which are in DANGLE.
PUSH R4
PUSHF AC1
MOV SRVDAT,DAT
JSR PC,MOVPUM ;Move arm to angles in DANGLE
POPF AC1
POP R4
TST R1 ;If error during move, quit
BNE 50$
SLEEP #10. ; otherwise sleep
SOB R4,10$ ;Do as many times as there are segments.
50$: POPF AC1
POPF AC0
POP <R4,R3,R2,R0>
RTS PC
⊗ ;
DATA
FLT6: .FLT2 6.0
FLT15: .FLT2 15.0
FLT10: .FLT2 10.0
CODE
; JNTRTN: Joint motions via the keyboard.
; Makes the VT05 act as a joint-mode teachbox (joystick).
JNTRTN: OUTSTX JMSG1 ;TYPE MESSAGE
CLR MOVFLG ;0=ARM IS NOT MOVING
CLR CHRCNT ;RESET # OF CHARS TYPED.
LDF JINTSP,AC0 ;Re-calculate degrees/cycle in case it got changed
STF AC0,JNTSPD ;Joint speed - save it.
DIVF CYCRAT,AC0 ; Degrees/Cycle = Degrees/sec / Cycles/sec
STF AC0,JDEGR ;Store degrees per cycle here.
MOV DANGLE,R0 ;Where to put joint angles
MOV SRVDAT,DAT
JSR PC,PANGLE ;Read encoders, convert to angles, store in DANGLE
JGET: JSR PC,INCHRI ;any new command typed?
TST R0
BEQ 50$ ;if not, branch
INC CHRCNT ;ADD TO # OF CHARS ON THE LINE
CMP CHRCNT,CHRMAX
BLE 3$
PUSH R0
OUTSTX CRLFX ;do a crlf
CLR CHRCNT
POP R0
3$: MOV #JNTTBL,R1 ;SET R1 TO 1ST ENTRY IN CMD TABLE
MOVB R0,JNTEND ;SET LAST ENTRY TO THE CMD LETTER (FOR A TRAP)
5$: CMPB (R1),R0 ;DOES IT MATCH THE INPUT?
BEQ 10$ ;IF SO, QUIT
ADD #6,R1 ;TO NEXT ENTRY
BR 5$
10$: MOV 2(R1),R0 ;GET ADDR OF ROUTINE
CMP R0,#-1 ;IF NOT FOUND IN TABLE
BNE 20$ ;THEN COMPLAIN NOW
OUTSTX WHAT
CLR MOVFLG ;ARM IS NOT MOVING ANY MORE
BR JGET
20$: TST 4(R1) ;IS THIS A MOTION COMMAND? 0=NO (ROUTINE)
BNE 30$ ;YES - GO SET UP DIRECTION VECTOR
JMP (R0) ;NO - GO TO ROUTINE
30$: MOV #JNTDIR,R0 ;NOW SET UP DIRECTION VECTOR. R0 TO VECTOR
MOV 4(R1),R2 ;PUT JOINT NUMBER IN R0 (FROM TABLE)
MOV #1,R1 ;R1 COUNTS THE JOINT NUMBER
35$: CLRF (R0) ;ASSUME ZERO
CMP R1,R2 ;IS THIS THE DIRECTION?
BNE 40$ ;NO - CONTINUE
MOV JDEGR,(R0) ;COPY DEGREES/SEGMENT INTO DIRECTION COMPONENT
MOV JDEGR+2,2(R0)
40$: CMP (R0)+,(R0)+ ;INCREMENT POINTER
INC R1 ;AND COUNTER
CMP R1,#6 ;END?
BLE 35$ ;NO - KEEP GOING
CLR MOVFLG ;MEANS ARM IS STOPPED.
MOV #1,JDIREC ;MOVE IN POSITIVE DIRECTION.
BR JGET ;Wait for + or - motion command before we move.
50$: TST MOVFLG ;IS THE ARM MOVING? IF NOT,
BEQ JGET ; then keep look till we get a command
BR JDOIT ; otherwise keep moving it.
JNEG: MOV #-1,JDIREC ;Means joint should go in - direction
BR JMOVE
JPOS: MOV #1,JDIREC ;Means joint should go in + direc.
JMOVE: INC MOVFLG ;Means arm is moving now.
MOV #JNTDIR,R0 ;Now make direction the right way.
MOV #NJTS,R1
3$: LDF (R0),AC0 ;Get current dir.
ABSF AC0 ;assume positive
TST JDIREC ;negative motion?
BPL 5$ ; no - br
NEGF AC0 ; yes - make it negative.
5$: STF AC0,(R0)+ ;
SOB R1,3$
JDOIT: MOV DANGLE,R0
MOV #NJTS,R1 ;Now add angle increments to DANGLE
MOV #JNTDIR,R2
1$: LDF @(R0),AC0
ADDF (R2),AC0
STF AC0,@(R0)+
CMP (R2)+,(R2)+
SOB R1,1$
MOV DANGLE,R0 ;SET ADDR OF ANGLES TO MOVE TO
MOV SRVDAT,DAT
JSR PC,MOVPUM ;MOVE THE ARM TO DESIRED POSITION.
TST R1
BNE JQUIT ;IF ERROR DURING MOVE, QUIT
SLEEP #16. ;SLEEP FOR 16 msec
JMP JGET
; [JNTRTN: CONTINUATION]
JSLOW: LDF FLT1,AC1 ;CALCULATE 1/JFACTR
DIVF JFACTR,AC1 ;FACTOR TO MULTIPLY BY INTO AC1.
BR JCALC
JFAST: LDF JFACTR,AC1 ;MULTIPLICATION FACTOR
JCALC: LDF JNTSPD,AC0 ;INCREASE/DECREASE THE SPEED
MULF AC1,AC0
STF AC0,JNTSPD ;Store new joint speed.
DIVF CYCRAT,AC0 ;Angle increment = JointSpeed / CycleRate
STF AC0,JDEGR
MOV #JNTDIR,R0 ;Now multiply direction vector by factor
MOV #NJTS,R1
5$: LDF (R0),AC0 ;Get current component
MULF AC1,AC0 ;Multiply by factor
STF AC0,(R0)+
SOB R1,5$
JMP JGET
JOPEN: MOV #AIROPN,R0 ;Set bit to open hand
BR JHAND
JCLOSE: MOV #AIRCLS,R0 ;Bit to close hand
JHAND: MOV SRVDAT,DAT
BIC #AIROPN+AIRCLS,ARMBIT(DAT)
CALL BITON ;Set bit in R0 on in joint 7 status word
JMP JGET
JSTOP: CLR MOVFLG ;NO LONGER MOVING
JMP JGET
JQUIT: CLR R1
RTS PC ;EXIT
DATA
JNTTBL: .ASCIZ /A/ ;COMMAND = A = SLOW DOWN
.WORD JSLOW,0 ;ROUTINE ADDR & 0=NOT MOTION COMMAND.
.ASCIZ /F/
.WORD JNEG,0 ;MOVE NEGATIVE
.ASCIZ /J/
.WORD JPOS,0 ;MOVE POSITIVE
.ASCIZ /;/
.WORD JFAST,0 ;MOVE FASTER
.ASCIZ /Q/
.WORD JQUIT,0
.ASCIZ / /
.WORD JSTOP,0
.ASCIZ /O/
.WORD JOPEN,0
.ASCIZ /C/
.WORD JCLOSE,0
.ASCIZ /1/ ;1=MOVE JOINT 1
.WORD 0,1
.ASCIZ /2/
.WORD 0,2
.ASCIZ /3/
.WORD 0,3
.ASCIZ /4/
.WORD 0,4
.ASCIZ /5/
.WORD 0,5
.ASCIZ /6/
.WORD 0,6
JNTEND: .WORD 0,-1,0 ;END OF TABLE
JINTSP: .FLT2 4.0 ;INITIAL JOINT SPEED (DEGR/SEC)
JFACTR: .FLT2 1.6 ;SPEED CHANGE FACTOR
JDEGR: .FLT2 0.0 ;DEGREES PER CYCLE
JNTDIR: .BLKW <2*NJTS> ;DIRECTION OF MOTION
JDIREC: .BLKW 1 ;1 OR -1 FOR POSITIVE/NEGATIVE MOTION
JNTSPD: .FLT2 2.0 ;JOINT SPEED - DEGREES/SEC.
CHRCNT: .WORD 0
CHRMAX: .WORD 30.
CYCRAT: .FLT2 60.0 ;HOW MANY CYCLES PER SECOND
MOVFLG: .WORD 0 ;1=puma moving
WHAT: .ASCIZ /?? /
JMSG1: .ASCIZ /
JOINT CONTROL MODE: /
BADJT: .ASCIZ /
BAD JOINT/
.EVEN
CODE
; FRERTN: Frees up a joint
; this routine garbages R0 and R1
FRERTN: OUTSTX FREMS1 ;ASK WHICH JOINT TO FREE UP
JSR PC,@LINCHR ; R1←response
PUSH R1
MOV R1,R0
JSR PC,@LTYPCHR ; echo it
OUTSTX CRLFX
POP R1
SUB #60,R1 ;MAKE IT AN INTEGER.
CMP R1,#1 ;VALIDATE THE INPUT
BLT 10$
CMP R1,#6
BLE 20$
10$: OUTSTX FREMS2 ;INVALID INPUT message
BR 100$
20$: DEC R1 ; transform 1 - 6 to 0-5
ADD #10,R1 ;ADD 10 ==> CURRENT MODE
CLR R0 ;DATA = 0 ==> NO TORQUE
MOV SRVDAT,DAT
JSR PC,WSINGL ;FREE UP THE JOINT!
OUTSTX FREMS3 ; print type any char to stop
JSR PC,@LINCHR ;R1←charcter
MOV R1,R0
JSR PC,@LTYPCHR ;echo it
OUTSTX CRLFX
JSR PC,HLDPUM ; hold puma in current position
OUTSTX FREMS4
100$: OUTSTX CRLFX
RTS PC
DATA
FREMS1: .ASCIZ /
FREE UP JOINT# (1-6): /
FREMS2: .ASCIZ /
SORRY, BAD JOINT #/
FREMS3: .ASCIZ /
TYPE ANY CHAR TO STOP: /
FREMS4: .ASCIZ /
-- STOP FREE./
CRLFX: .ASCIZ /
/
.EVEN
CODE
; GRARTN: Gravity model experimentation.
GRARTN: OUTSTX GRAMS1 ;Type informative msg
OUTSTX GRAMS2 ;Say arm is free (even though it isn't yet)
10$: MOV DANGLE,R0 ;Read joint angles & put in dangle
MOV SRVDAT,DAT
JSR PC,PANGLES
JSR PC,INCHRI ; see if character typed
TST R0 ;Was a char typed?
BNE 50$ ;If so, quit now.
JSR PC,GCALC ;Calculate torques needed to keep arm steady
MOV #TRQUE,R0 ;Set address of torques to apply
MOV #CURRNT+SEQMDE,R1 ;Current mode for all joints
MOV SRVDAT,DAT
JSR PC,WVECT ;Enter current mode!
SLEEP #10. ;Wait a little while (10 msec)
BR 10$ ;Do it again.
50$: JSR PC,HLDPUM ; hold pumas in current position
OUTSTX GRAMS3 ;Type msg upon return.
GRARET: RTS PC
JJ1==TH1*2
JJ2==TH2*2
JJ3==TH3*2
JJ4==TH4*2
JJ5==TH5*2
JJ6==TH6*2
GCALC: CLRF KANGLE+JJ1 ;No torque for joint 1
MOV DANGLE,R0
LDF @TH2(R0),AC0 ;For joint 2, torque = Ag*sin2 + Bg*sin23
ADDF FLT90,AC0 ;Account for different joint angle offset
JSR PC,SNCOS
STF AC0,GSIN2
LDF @TH3(R0),AC0 ;Theta3 = theta3 - 90
ADDF @TH2(R0),AC0 ;Theta23 = (theta3-90) + (theta2+90) = theta2+3
JSR PC,SNCOS
STF AC0,GSIN23
LDF GSIN2,AC0 ;Now calculate torque as above.
MULF GRAG,AC0
LDF GSIN23,AC1
MULF GRBG,AC1
ADDF AC1,AC0
STF AC0,KANGLE+JJ2 ;Store torque for joint 2
LDF GSIN23,AC0 ;Now calculate torque for joint 3.
MULF GRBG,AC0 ;Torque = B*g*sin23
STF AC0,KANGLE+JJ3
CLRF KANGLE+JJ4 ;No torque for the others joints.
CLRF KANGLE+JJ5
CLRF KANGLE+JJ6
MOV #NJTS,R3 ;Now convert torques into DAC units using
CLR R0 ; scale factors TSCALE.
CLR R1
10$: LDF KANGLE(R0),AC0 ;Get torque in oz-in.
MULF TSCALE(R0),AC0 ;Multiply by DAC/oz-in to get DAC's
STCFI AC0,TRQUE(R1) ;Put into final place
CMP (R0)+,(R0)+ ; (bump pointers)
TST (R1)+
SOB R3,10$
RTS PC
DATA
GRAMS1: .ASCIZ /
GRAVITY MODEL TESTER./
GRAMS2: .ASCIZ /
ARM SHOULD BE FREE NOW...TYPE ANY CHAR TO STOP: /
GRAMS3: .ASCIZ /
ALL DONE./
.EVEN
GRAG: .FLT2 16.5 ;This is A*g in JKS's model
GRBG: .FLT2 5.0 ;This is B*g in JKS's model
GSIN2: .FLT2 0.0 ;Sin(theta2)
GSIN23: .FLT2 0.0 ;Sin(theta2+theta3)
TRQUE: .WORD 0,0,0,0,0,0 ;TORQUES (FOR FORCE CONTROL MODE)
KANGLE: .FLT2 0,0,0,0,0,0 ;WORK AREA
TSCALE: .FLT2 9.2, 15.7, -11.0, 0.0, 0.0, 0.0 ;Torque scale factors
;(in DAC units per oz-in.)
CODE
DATA
DANGLE: .WORD GTH ;pointer to angle list
MECHNO: .WORD GRNARM
SRVDAT: .WORD SRV17
CANGLES:.WORD INITH,INITH+4,INITH+10,INITH+14,INITH+20,INITH+24,INITH+30
INITH: .BLKW 14.
FTH: .BLKW 14.
DIFF: .BLKW 14.
CODE